
import edu.wsu.KheperaSimulator.RobotController;

/**
 * 
 * @author Nixxx
 * 
 * This class provides basic behavior methods, that can
 * be used in every derived subsumption controller
 *
 */

public abstract class Subsumption extends RobotController {

	/**
	 * Flags for behavior decision
	 */
	protected boolean m_cruiserFlag;
	protected boolean m_wcheckerFlag;
	protected boolean m_bfinderFlag;
	protected boolean m_bpusherFlag;

	/**
	 * Motor speeds for each behavior
	 */
	protected int[] m_motorSpeeds = { 0, 0 };
	protected int[] m_cruiseOutput = { 0, 0 };
	protected int[] m_wcheckerOutput = { 0, 0 };
	protected int[] m_bfinderOutput = { 0, 0 };
	protected int[] m_bpusherOutput = { 0, 0 };

	/**
	 * Variable for storing the actual milage in every individual
	 * doWork() cycle
	 */
	protected long m_milage;

	/**
	 * Variables for storing the distance sensor values
	 */
	protected int s0, s1, s2, s3, s4, s5, s6, s7;
	
	/**
	 * Variables for storing the light sensor values
	 */
	protected int l0, l1, l2, l3, l4, l5, l6, l7;

	/**
	 * Different Thresholds for distance sensors
	 * derived from sensor max which should be
	 * found out at first
	 */
	protected final int SENSOR_MAX = 1020;
	protected final int LOW_THRESHOLD = SENSOR_MAX / 20;
	protected final int MID_THRESHOLD = SENSOR_MAX / 2;
	protected final int HIGH_THRESHOLD = (int) (SENSOR_MAX / 1.4);

	/**
	 * Different Thresholds for light sensors
	 * derived from sensor max which should be
	 * found out at first
	 */
	protected final int LSENSOR_MAX = 500;
	protected final int LOW_LIGHTTHRESHOLD = (int)(SENSOR_MAX / 5);
	protected final int MID_LIGHTTHRESHOLD = (int)(SENSOR_MAX / 2.5);
	protected final int HIGH_LIGHTTHRESHOLD = (int)(SENSOR_MAX / 1.25);

	public void doWork() throws Exception {

	}


	public void close() throws Exception {

	}

	/**
	 * abstract method for task individual priority implementation
	 */
	protected abstract void arbitrate();

	/**
	 * method for cruising straight forward if no sensor reacts
	 */
	protected void cruise() {

		getSensorValues();
		
		if (nothingAround()) {
			m_cruiseOutput[0] = 8;
			m_cruiseOutput[1] = 8;
			m_cruiserFlag = true;
		} else {
			m_cruiserFlag = false;
		}
	}

	/**
	 * method that checks if wall is around, avoiding that robot gets stuck
	 */
	protected void wallChecker() {

		getSensorValues();

		// leftWall and robot got stuck
		if ((s0 > LOW_THRESHOLD && s1 > LOW_THRESHOLD && s2 > LOW_THRESHOLD)
				&& (m_milage == getActualMilage())) {
			m_wcheckerOutput[0] = -1;
			m_wcheckerOutput[1] = -6;
			m_wcheckerFlag = true;
		// rightWall and robot got stuck
		} else if ((s3 > LOW_THRESHOLD && s4 > LOW_THRESHOLD && s5 > LOW_THRESHOLD)
				&& (m_milage == getActualMilage())) {
			m_wcheckerOutput[0] = -6;
			m_wcheckerOutput[1] = -1;
			m_wcheckerFlag = true;
		// frontWall and robot got stuck
		} else if ((s1 > LOW_THRESHOLD && s2 > LOW_THRESHOLD
				&& s3 > LOW_THRESHOLD && s4 > LOW_THRESHOLD)
				&& (m_milage == getActualMilage())) {
			m_wcheckerOutput[0] = -6;
			m_wcheckerOutput[1] = -1;
			m_wcheckerFlag = true;
			// backWall and robot got stuck
		} else if ((s6 >= HIGH_THRESHOLD || s7 >= HIGH_THRESHOLD)
				&& (m_milage == getActualMilage())) {
			m_wcheckerOutput[0] = 5;
			m_wcheckerOutput[1] = 5;
			m_wcheckerFlag = true;
		/*} else if (getLightValue(2) < 100 || getLightValue(3) < 100
				|| getLightValue(1) < 100 || getLightValue(4) < 100) {
			System.out.println("Close to light");
			m_wcheckerOutput[0] = -6;
			m_wcheckerOutput[1] = -1;
			m_wcheckerFlag = true;*/
		} else {
			m_wcheckerFlag = false;
		}
	}

	/**
	 * if a ball is at robot front, it's being pushed straight forward
	 */
	protected void ballPusher() {
		getSensorValues();
		if ((s2 >= SENSOR_MAX || s3 >= SENSOR_MAX)
				&& (m_milage != getActualMilage())) {
			m_bpusherOutput[0] = 8;
			m_bpusherOutput[1] = 8;
			m_bpusherFlag = true;
		} else {
			m_bpusherFlag = false;
		}
	}
    /**
     * method for finding and moving towards a ball, if any front
     * distance sensor gets input
     */
	protected void ballFinder() {
		getSensorValues();
		
		//if ball in front
		if ((s2 > LOW_THRESHOLD || s3 > LOW_THRESHOLD) && nothingLeft()
				&& nothingRight()) {
			// move towards ball
			if (s2 > s3) {
				m_bfinderOutput[0] = 0;
				m_bfinderOutput[1] = 2;
			} else if (s3 > s2) {
				m_bfinderOutput[0] = 2;
				m_bfinderOutput[1] = 0;
			} else {
				m_bfinderOutput[0] = 2;
				m_bfinderOutput[1] = 2;
			}
			m_bfinderFlag = true;
		// Ball at Sensor s0
		} else if (s0 > LOW_THRESHOLD && s1 < LOW_THRESHOLD && nothingInFront()
				&& nothingRight()) {
			m_bfinderOutput[0] = -4;
			m_bfinderOutput[1] = 4;
			m_bfinderFlag = true;
		// Ball at Sensor s5
		} else if (s5 > LOW_THRESHOLD && s4 < LOW_THRESHOLD && nothingInFront()
				&& nothingLeft()) {
			m_bfinderOutput[0] = 4;
			m_bfinderOutput[1] = -4;
			m_bfinderFlag = true;
		// Ball at Sensor s1
		} else if (s1 > LOW_THRESHOLD && s0 < LOW_THRESHOLD && nothingInFront()
				&& nothingRight()) {
			m_bfinderOutput[0] = -3;
			m_bfinderOutput[1] = 3;
			m_bfinderFlag = true;
		// Ball at Sensor s4
		} else if (s4 > LOW_THRESHOLD && s5 < LOW_THRESHOLD && nothingInFront()
				&& nothingLeft()) {
			m_bfinderOutput[0] = 3;
			m_bfinderOutput[1] = -3;
			m_bfinderFlag = true;
			
		// Ball at Sensor s0 + s1
		} else if (s0 > LOW_THRESHOLD && s1 < MID_THRESHOLD && nothingInFront()
				&& nothingRight()) {
			m_bfinderOutput[0] = -4;
			m_bfinderOutput[1] = 4;
			m_bfinderFlag = true;
		// Ball at Sensor s5 + s4
		} else if (s5 > LOW_THRESHOLD && s4 < MID_THRESHOLD && nothingInFront()
				&& nothingLeft()) {
			m_bfinderOutput[0] = 4;
			m_bfinderOutput[1] = -4;
			m_bfinderFlag = true;
		// Ball at Sensor s1 + s0
		} else if (s1 > LOW_THRESHOLD && s0 < MID_THRESHOLD && nothingInFront()
				&& nothingRight()) {
			m_bfinderOutput[0] = -3;
			m_bfinderOutput[1] = 3;
			m_bfinderFlag = true;
		// Ball at Sensor s4 + s5
		} else if (s4 > LOW_THRESHOLD && s5 < MID_THRESHOLD && nothingInFront()
				&& nothingLeft()) {
			m_bfinderOutput[0] = 3;
			m_bfinderOutput[1] = -3;
			m_bfinderFlag = true;

		} else {
			m_bfinderFlag = false;
		}
	}

	/**
	 * Method for checking if something solid is at left side
	 * @return true if nothing solid is at left side
	 * @return false if something solid is at left side
	 */
	protected boolean nothingLeft() {
		if ((s0 < LOW_THRESHOLD) && (s1 < LOW_THRESHOLD)) {
			return true;
		} else {
			return false;
		}
	}

	/**
	 * Method for checking if something solid is at right side
	 * @return true if nothing solid is at right side
	 * @return false if something solid is at right side
	 */
	protected boolean nothingRight() {
		if ((s4 < LOW_THRESHOLD) && (s5 < LOW_THRESHOLD)) {
			return true;
		} else {
			return false;
		}
	}

	/**
	 * Method for checking if something solid is at front side
	 * @return true if nothing solid is at front side
	 * @return false if something solid is at front side
	 */
	protected boolean nothingInFront() {
		if ((s2 < LOW_THRESHOLD) && (s3 < LOW_THRESHOLD)) {
			return true;
		} else {
			return false;
		}
	}

	/**
	 * Method for checking if something solid is around robot
	 * @return true if nothing solid is around robot
	 * @return false if something solid is around robot
	 */
	protected boolean nothingAround() {
		if ((s0 < LOW_THRESHOLD) && (s1 < LOW_THRESHOLD)
				&& (s2 < LOW_THRESHOLD) && (s3 < LOW_THRESHOLD)
				&& (s4 < LOW_THRESHOLD) && (s5 < LOW_THRESHOLD)) {
			return true;
		} else {
			return false;
		}
	}

	/**
	 * Method for getting and storing the actual distance sensor values
	 */
	protected void getSensorValues() {
		s0 = getDistanceValue(0);
		s1 = getDistanceValue(1);
		s2 = getDistanceValue(2);
		s3 = getDistanceValue(3);
		s4 = getDistanceValue(4);
		s5 = getDistanceValue(5);
		s6 = getDistanceValue(6);
		s7 = getDistanceValue(7);
	}
	
	/**
	 * Method for getting and storing the actual light sensor values
	 */
	protected void getLightValues() {
		l0 = getLightValue(0);
		l1 = getLightValue(1);
		l2 = getLightValue(2);
		l3 = getLightValue(3);
		l4 = getLightValue(4);
		l5 = getLightValue(5);
		l6 = getLightValue(6);
		l7 = getLightValue(7);
	}

	/**
	 * Method for getting and storing the actual milage that robot traveled so far
	 */
	protected void setMilage() {
		m_milage = getRightWheelPosition();
	}

	/**
	 * 
	 * @return the actual milage that robot traveled so far
	 */
	protected long getActualMilage() {
		long milage = getRightWheelPosition();
		return milage;
	}
}
